/*************************************************************
 Copyright (C) 2022 Earth Rover Limited. All rights reserved.
*************************************************************/

/**
 * @file localization_status.h
 * @brief Header for the Localization Status Class
 * @date July 2022
 */

#ifndef __STATUS_H__
#define __STATUS_H__

// C++ standard libraries
#include <string>
#include <iostream>

#include <ros/ros.h>
#include <earth_rover_common_messages/LocalizationStatus.h>
#include <piksi_rtk_msgs/ReceiverState_V2_4_1.h>
#include <piksi_rtk_msgs/BaselineHeading.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>

// Declaration of constants for GNSS ATTITUDE
int INT_GNSS_ATTITUDE_INVALID    = 0;
int INT_GNSS_ATTITUDE_RESERVED   = 1;
int INT_GNSS_ATTITUDE_DGNSS      = 2;
int INT_GNSS_ATTITUDE_FLOAT_RTK  = 3;
int INT_GNSS_ATTITUDE_FIXED_RTK  = 4;

class Status
{
    public:
        Status(const ros::NodeHandle &nh);
        void gnssReferenceCb(const piksi_rtk_msgs::ReceiverState_V2_4_1& msg);
        void gnssAttitudeCb(const piksi_rtk_msgs::BaselineHeading& msg);
        void wheelOdometryCb(const nav_msgs::Odometry& msg);
        void imuCb(const sensor_msgs::Imu& msg);

        void statusPublisherCb(const ros::TimerEvent& e);
        void watchdogGnssReferenceCb(const ros::TimerEvent& e);
        void watchdogGnssAttitudeCb(const ros::TimerEvent& e);
        void watchdogWheelOdometryCb(const ros::TimerEvent& e);
        void watchdogImuCb(const ros::TimerEvent& e);

    private:
        float m_timeout_gnss_ref, m_timeout_gnss_att, m_timeout_wheel_odom, m_timeout_imu;
        ros::Time m_watchdog_time_gnss_ref, m_watchdog_time_gnss_att, m_watchdog_time_wheel_odom, m_watchdog_time_imu;
        bool m_gnss_ref_messages_started = false, m_gnss_att_messages_started = false, m_wheel_odom_messages_started = false, m_imu_messages_started = false;

        ros::NodeHandle m_nh;
        ros::Publisher m_status_pub;
        ros::Subscriber m_gnss_ref_sub, m_gnss_att_sub, m_wheel_odom_sub, m_imu_sub;

        ros::Timer m_timer_status_pub, m_timer_gnss_ref, m_timer_gnss_att, m_timer_wheel_odom, m_timer_imu;
        earth_rover_common_messages::LocalizationStatus m_localization_status;
};

#endif
